#include "TBMat4x4.h"
#include "TBMat3x3.h"

namespace tb
{

	// ================================
	// Constructors
	// ================================

	TB_INLINE Quat::Quat() 
	{
		x = y = z = 0.f;
		w = 1.f;
	}

	TB_INLINE Quat::Quat(float a_X, float a_Y, float a_Z)
	{
		Quat qx(1.f, 0.f, 0.f, a_X);
		Quat qy(0.f, 1.f, 0.f, a_Y);
		Quat qz(0.f, 0.f, 1.f, a_Z);

		*this = qx;
		*this *= qy;
		*this *= qz;
	}

	TB_INLINE Quat::Quat(float a_X, float a_Y, float a_Z, float a_W) 
	{
		x = a_X;
		y = a_Y;
		z = a_Z;
		w = a_W;
	}

	TB_INLINE Quat::Quat(const Vec3& a_Axis, float a_Degrees) 
	{
		// normalize

		Vec3 norm = a_Axis.GetNormalized();

		// set values

		float angle = Math::SinDeg(a_Degrees * 0.5f);
		x = norm.x * angle;
		y = norm.y * angle;
		z = norm.z * angle;
		w = Math::CosDeg(a_Degrees * 0.5f);
	}

	// ================================
	// Functions
	// ================================

	TB_INLINE void Quat::Identity()
	{
		x = y = z = 0;
		w = 1;
	}

	TB_INLINE float Quat::GetDotProduct(const Quat& a_Other) const
	{
		return (x * a_Other.x) + (y * a_Other.y) + (z * a_Other.z) + (w * a_Other.w);
	}

	TB_INLINE float Quat::GetSquaredLength() const
	{
		return (x * x + y * y + z * z + w * w);
	}

	TB_INLINE float Quat::GetLength() const
	{
		return (Math::Sqrt(x * x + y * y + z * z + w * w));
	}

	TB_INLINE Quat& Quat::Normalize()
	{
		float length = 1.f / GetLength();
		x *= length;
		y *= length;
		z *= length;
		w *= length;

		return *this;
	}

	TB_INLINE Quat Quat::GetNormalized() const
	{
		float length = 1.f / GetLength();

		return Quat(
			x * length,
			y * length,
			z * length,
			w * length
		);
	}

	TB_INLINE Vec3 Quat::GetDirection() const
	{
		float length = 1.f / GetLength();
		float xn = x * length;
		float yn = y * length;
		float zn = z * length;
		float wn = w * length;

		return (
			Vec3(
				(2.f * (xn * zn - wn * yn)),
				(2.f * (yn * zn + wn * xn)),
				(1.f - 2.f * (xn * xn + yn * yn))
			)
		);
	}

	TB_INLINE Vec3 Quat::GetTangent() const
	{
		Quat final = *this;
		final *= Quat(0.f, -90.f, 0.f);

		return final.GetDirection();
	}

	TB_INLINE Vec3 Quat::GetBitangent() const
	{
		Quat final = *this;
		final *= Quat(90.f, 0.f, 0.f);

		return final.GetDirection();
	}

	TB_INLINE Quat& Quat::Conjugate()
	{
		x = -x;
		y = -y;
		z = -z;

		return *this;
	}

	TB_INLINE Quat Quat::GetConjugate() const
	{
		return (Quat(-x, -y, -z, w));
	}

	// ================================
	// Conversion
	// ================================

	TB_INLINE Mat4x4 Quat::ToMat4x4() const
	{
		Mat4x4 result;

		float xn = x + x;
		float yn = y + y;
		float zn = z + z;

		const float xx = xn * x;
		const float xw = xn * w;

		const float yx = yn * x;
		const float yy = yn * y;
		const float yw = yn * w;

		const float zx = zn * x;
		const float zy = zn * y;
		const float zz = zn * z;
		const float zw = zn * w;

		result.values[Mat4x4::X1] = 1.f - (yy + zz);
		result.values[Mat4x4::Y1] = yx + zw;
		result.values[Mat4x4::Z1] = zx - yw;
		result.values[Mat4x4::WX] = 0.f;

		result.values[Mat4x4::X2] = yx - zw;
		result.values[Mat4x4::Y2] = 1.f - (xx + zz);
		result.values[Mat4x4::Z2] = zy + xw;
		result.values[Mat4x4::WY] = 0.f;

		result.values[Mat4x4::X3] = zx + yw;
		result.values[Mat4x4::Y3] = zy - xw;
		result.values[Mat4x4::Z3] = 1.f - (xx + yy);
		result.values[Mat4x4::WZ] = 0.f;

		result.values[Mat4x4::TX] = 0.f;
		result.values[Mat4x4::TY] = 0.f;
		result.values[Mat4x4::TZ] = 0.f;
		result.values[Mat4x4::CZ] = 1.f;

		return result;
	}

	TB_INLINE Mat4x4 Quat::ToMat4x4(float a_X, float a_Y, float a_Z) const
	{
		Mat4x4 result;

		float xn = x + x;
		float yn = y + y;
		float zn = z + z;

		const float xx = xn * x;
		const float xw = xn * w;

		const float yx = yn * x;
		const float yy = yn * y;
		const float yw = yn * w;

		const float zx = zn * x;
		const float zy = zn * y;
		const float zz = zn * z;
		const float zw = zn * w;

		result.values[Mat4x4::X1] = 1.f - (yy + zz);
		result.values[Mat4x4::Y1] = yx + zw;
		result.values[Mat4x4::Z1] = zx - yw;
		result.values[Mat4x4::WX] = 0.f;

		result.values[Mat4x4::X2] = yx - zw;
		result.values[Mat4x4::Y2] = 1.f - (xx + zz);
		result.values[Mat4x4::Z2] = zy + xw;
		result.values[Mat4x4::WY] = 0.f;

		result.values[Mat4x4::X3] = zx + yw;
		result.values[Mat4x4::Y3] = zy - xw;
		result.values[Mat4x4::Z3] = 1.f - (xx + yy);
		result.values[Mat4x4::WZ] = 0.f;

		result.values[Mat4x4::TX] = a_X;
		result.values[Mat4x4::TY] = a_Y;
		result.values[Mat4x4::TZ] = a_Z;
		result.values[Mat4x4::CZ] = 1.f;

		return result;
	}

	TB_INLINE Mat4x4 Quat::ToMat4x4(const Vec3& a_Translation) const
	{
		Mat4x4 result;

		float xn = x + x;
		float yn = y + y;
		float zn = z + z;

		const float xx = xn * x;
		const float xw = xn * w;

		const float yx = yn * x;
		const float yy = yn * y;
		const float yw = yn * w;

		const float zx = zn * x;
		const float zy = zn * y;
		const float zz = zn * z;
		const float zw = zn * w;

		result.values[Mat4x4::X1] = 1.f - (yy + zz);
		result.values[Mat4x4::Y1] = yx + zw;
		result.values[Mat4x4::Z1] = zx - yw;
		result.values[Mat4x4::WX] = 0.f;

		result.values[Mat4x4::X2] = yx - zw;
		result.values[Mat4x4::Y2] = 1.f - (xx + zz);
		result.values[Mat4x4::Z2] = zy + xw;
		result.values[Mat4x4::WY] = 0.f;

		result.values[Mat4x4::X3] = zx + yw;
		result.values[Mat4x4::Y3] = zy - xw;
		result.values[Mat4x4::Z3] = 1.f - (xx + yy);
		result.values[Mat4x4::WZ] = 0.f;

		result.values[Mat4x4::TX] = a_Translation.x;
		result.values[Mat4x4::TY] = a_Translation.y;
		result.values[Mat4x4::TZ] = a_Translation.z;
		result.values[Mat4x4::CZ] = 1.f;

		return result;
	}

	TB_INLINE Mat4x4 Quat::ToCameraMatrix(const Vec3& a_Translation) const
	{
		Mat4x4 result;

		float xn = x + x;
		float yn = y + y;
		float zn = z + z;

		const float xx = xn * x;
		const float xw = xn * w;

		const float yx = yn * x;
		const float yy = yn * y;
		const float yw = yn * w;

		const float zx = zn * x;
		const float zy = zn * y;
		const float zz = zn * z;
		const float zw = zn * w;

		result.values[Mat4x4::X1] = 1.f - (yy + zz);
		result.values[Mat4x4::Y1] = yx - zw;
		result.values[Mat4x4::Z1] = zx + yw;
		result.values[Mat4x4::WX] = 0.f;

		result.values[Mat4x4::X2] = yx + zw;
		result.values[Mat4x4::Y2] = 1.f - (xx + zz);
		result.values[Mat4x4::Z2] = zy - xw;
		result.values[Mat4x4::WY] = 0.f;

		result.values[Mat4x4::X3] = zx - yw;
		result.values[Mat4x4::Y3] = zy + xw;
		result.values[Mat4x4::Z3] = 1.f - (xx + yy);
		result.values[Mat4x4::WZ] = 0.f;

		float x = -a_Translation.x;
		float y = -a_Translation.y;
		float z = -a_Translation.z;

		result.values[Mat4x4::TX] = (x * result.values[Mat4x4::X1]) + (y * result.values[Mat4x4::X2]) + (z * result.values[Mat4x4::X3]);
		result.values[Mat4x4::TY] = (x * result.values[Mat4x4::Y1]) + (y * result.values[Mat4x4::Y2]) + (z * result.values[Mat4x4::Y3]);
		result.values[Mat4x4::TZ] = (x * result.values[Mat4x4::Z1]) + (y * result.values[Mat4x4::Z2]) + (z * result.values[Mat4x4::Z3]);
		result.values[Mat4x4::CZ] = 1.f;

		return result;
	}

	TB_INLINE Mat4x4 Quat::ToCameraMatrix(float a_X, float a_Y, float a_Z) const
	{
		Mat4x4 result;

		float xn = x + x;
		float yn = y + y;
		float zn = z + z;

		const float xx = xn * x;
		const float xw = xn * w;

		const float yx = yn * x;
		const float yy = yn * y;
		const float yw = yn * w;

		const float zx = zn * x;
		const float zy = zn * y;
		const float zz = zn * z;
		const float zw = zn * w;

		result.values[Mat4x4::X1] = 1.f - (yy + zz);
		result.values[Mat4x4::Y1] = yx - zw;
		result.values[Mat4x4::Z1] = zx + yw;
		result.values[Mat4x4::WX] = 0.f;

		result.values[Mat4x4::X2] = yx + zw;
		result.values[Mat4x4::Y2] = 1.f - (xx + zz);
		result.values[Mat4x4::Z2] = zy - xw;
		result.values[Mat4x4::WY] = 0.f;

		result.values[Mat4x4::X3] = zx - yw;
		result.values[Mat4x4::Y3] = zy + xw;
		result.values[Mat4x4::Z3] = 1.f - (xx + yy);
		result.values[Mat4x4::WZ] = 0.f;

		float x = -a_X;
		float y = -a_Y;
		float z = -a_Z;

		result.values[Mat4x4::TX] = (x * result.values[Mat4x4::X1]) + (y * result.values[Mat4x4::X2]) + (z * result.values[Mat4x4::X3]);
		result.values[Mat4x4::TY] = (x * result.values[Mat4x4::Y1]) + (y * result.values[Mat4x4::Y2]) + (z * result.values[Mat4x4::Y3]);
		result.values[Mat4x4::TZ] = (x * result.values[Mat4x4::Z1]) + (y * result.values[Mat4x4::Z2]) + (z * result.values[Mat4x4::Z3]);
		result.values[Mat4x4::CZ] = 1.f;

		return result;
	}

	TB_INLINE Quat& Quat::FromMatrix(const Mat4x4& a_Matrix)
	{
		//*this = a_Matrix.ToQuaternion();

		// TODO: Remove switch ugliness

		float value_x = a_Matrix.values[Mat4x4::X1] - a_Matrix.values[Mat4x4::Y2] - a_Matrix.values[Mat4x4::Z3];
		float value_y = a_Matrix.values[Mat4x4::Y2] - a_Matrix.values[Mat4x4::X1] - a_Matrix.values[Mat4x4::Z3];
		float value_z = a_Matrix.values[Mat4x4::Z3] - a_Matrix.values[Mat4x4::X1] - a_Matrix.values[Mat4x4::Y2];
		float value_w = a_Matrix.values[Mat4x4::X1] + a_Matrix.values[Mat4x4::Y2] + a_Matrix.values[Mat4x4::Z3];

		float biggest = value_w;
		int biggest_index = 3;

		if (value_x > biggest)
		{
			biggest = value_x;
			biggest_index = 0;
		}
		if (value_y > biggest)
		{
			biggest = value_y;
			biggest_index = 1;
		}
		if (value_z > biggest)
		{
			biggest = value_z;
			biggest_index = 2;
		}

		float biggest_value = Math::Sqrt(biggest + 1.f) * 0.5f;
		float mult = 0.25f / biggest_value;

		switch (biggest_index)
		{

		case 0:
			{
				x = biggest_value;
				y = (a_Matrix.values[Mat4x4::Y1] + a_Matrix.values[Mat4x4::X2]) * mult;
				z = (a_Matrix.values[Mat4x4::X3] + a_Matrix.values[Mat4x4::Z1]) * mult;
				w = (a_Matrix.values[Mat4x4::Z2] - a_Matrix.values[Mat4x4::Y3]) * mult;
				break;
			}
		case 1:
			{
				x = (a_Matrix.values[Mat4x4::Y1] + a_Matrix.values[Mat4x4::X2]) * mult;
				y = biggest_value;
				z = (a_Matrix.values[Mat4x4::Z2] + a_Matrix.values[Mat4x4::Y3]) * mult;
				w = (a_Matrix.values[Mat4x4::X3] - a_Matrix.values[Mat4x4::Z1]) * mult;
				break;
			}
		case 2:
			{
				x = (a_Matrix.values[Mat4x4::X3] + a_Matrix.values[Mat4x4::Z1]) * mult;
				y = (a_Matrix.values[Mat4x4::Z2] + a_Matrix.values[Mat4x4::Y3]) * mult;
				z = biggest_value;
				w = (a_Matrix.values[Mat4x4::Y1] - a_Matrix.values[Mat4x4::X2]) * mult;
				break;
			}

		case 3:
			{
				x = (a_Matrix.values[Mat4x4::Z2] - a_Matrix.values[Mat4x4::Y3]) * mult;
				y = (a_Matrix.values[Mat4x4::X3] - a_Matrix.values[Mat4x4::Z1]) * mult;
				z = (a_Matrix.values[Mat4x4::Y1] - a_Matrix.values[Mat4x4::X2]) * mult;
				w = biggest_value; 
				break;
			}
			
		}

		return *this;
	}

	TB_INLINE Quat& Quat::FromAxes(const Vec3& a_X, const Vec3& a_Y, const Vec3& a_Z)
	{
		// borrowed from Ogre

		/*float values[3][3] = { 
			{ a_X.x, a_X.y, a_X.z },
			{ a_Y.x, a_Y.y, a_Y.z }, 
			{ a_Z.x, a_Z.y, a_Z.z }
		};*/

		float values[3][3] = { 
			{ a_X.x, a_Y.x, a_Z.x },
			{ a_X.y, a_Y.y, a_Z.y }, 
			{ a_X.z, a_Y.z, a_Z.z }
		};

		float fTrace = values[0][0] + values[1][1] + values[2][2];
		float fRoot;

		if (fTrace > 0.f)
		{
			// |w| > 1/2, may as well choose w > 1/2
			fRoot = Math::Sqrt(fTrace + 1.f);  // 2w
			w = 0.5f * fRoot;
			fRoot = 0.5f / fRoot;  // 1/(4w)
			x = (values[2][1] - values[1][2]) * fRoot;
			y = (values[0][2] - values[2][0]) * fRoot;
			z = (values[1][0] - values[0][1]) * fRoot;
		}
		else
		{
			// |w| <= 1/2
			static size_t s_iNext[3] = { 1, 2, 0 };

			size_t i = 0;
			if (values[1][1] > values[0][0]) { i = 1; }
			if (values[2][2] > values[i][i]) { i = 2; }
			size_t j = s_iNext[i];
			size_t k = s_iNext[j];

			fRoot = Math::Sqrt(values[i][i] - values[j][j] - values[k][k] + 1.f);
			float* apkQuat[3] = { &x, &y, &z };
			*apkQuat[i] = 0.5f * fRoot;
			fRoot = 0.5f / fRoot;
			w = (values[k][j] - values[j][k]) * fRoot;
			*apkQuat[j] = (values[j][i] + values[i][j]) * fRoot;
			*apkQuat[k] = (values[k][i] + values[i][k]) * fRoot;
		}

		return *this;
	}

	TB_INLINE Quat& Quat::FromAngleAxis(const Vec3& a_Axis, float a_Angle)
	{
		float halfangle = 0.5f * a_Angle;

		float sintheta = Math::SinDeg(halfangle);
		x = sintheta * a_Axis.x;
		y = sintheta * a_Axis.y;
		z = sintheta * a_Axis.z;
		w = Math::CosDeg(halfangle);

		return *this;
	}

	// ================================
	// Rotation
	// ================================

	TB_INLINE tb::Vec3 Quat::Transform(const Vec3& a_Vector)
	{
		// from @mmalex
		// p+=2.f*cross(cross(p,q.xyz)+q.w*p,q.xyz); 

		Vec3 temp(
			(a_Vector.y * z) - (a_Vector.z * y) + (a_Vector.x * w),
			(a_Vector.z * x) - (a_Vector.x * z) + (a_Vector.y * w),
			(a_Vector.x * y) - (a_Vector.y * x) + (a_Vector.z * w)
		);

		return Vec3(
			a_Vector.x + ((temp.y * z) - (temp.z * y)) * 2.f,
			a_Vector.y + ((temp.z * x) - (temp.x * z)) * 2.f,
			a_Vector.z + ((temp.x * y) - (temp.y * x)) * 2.f
		);
	}

	// ================================
	// Interpolation
	// ================================

	TB_INLINE Quat& Quat::Slerp(const Quat& a_Other, float a_Percentage)
	{
		float angle_cos = GetDotProduct(a_Other);
		Quat y2 = a_Other;
		if (angle_cos < 0.f)
		{
			y2 *= -1;
			angle_cos = -angle_cos;
		}

		float k0, k1;
		if (angle_cos > 0.9999f)
		{
			k0 = a_Percentage;
			k1 = 1.f - a_Percentage;
		}
		else
		{
			float scale0 = 1.f - angle_cos * angle_cos;
			float angle_sin = 1.f / Math::Sqrt(scale0);
			float angle = Math::ArcTan2(scale0 * angle_sin, angle_cos);
			k0 = Math::Sin((1.f - a_Percentage) * angle) * angle_sin;
			k1 = Math::Sin((      a_Percentage) * angle) * angle_sin;
		}

		x = (k0 * x) + (k1 * y2.x);
		y = (k0 * y) + (k1 * y2.y);
		z = (k0 * z) + (k1 * y2.z);
		w = (k0 * w) + (k1 * y2.w);
		return *this;
	}

	TB_INLINE Quat Quat::GetSlerp(const Quat& a_Other, float a_Percentage) const
	{
		float angle_cos = GetDotProduct(a_Other);
		Quat y2 = a_Other;
		if (angle_cos < 0.f)
		{
			y2 *= -1;
			angle_cos = -angle_cos;
		}

		float k0, k1;
		if (angle_cos > 0.9999f)
		{
			k0 = a_Percentage;
			k1 = 1.f - a_Percentage;
		}
		else
		{
			float angle_sin = Math::Sqrt(1.f - angle_cos * angle_cos);
			float angle = Math::ArcTan2(angle_sin, angle_cos);
			float rcp_sin = 1.f / angle_sin;
			k0 = Math::Sin((1.f - a_Percentage) * angle) * rcp_sin;
			k1 = Math::Sin((      a_Percentage) * angle) * rcp_sin;
		}

		return Quat(
			(k0 * x) + (k1 * y2.x),
			(k0 * y) + (k1 * y2.y),
			(k0 * z) + (k1 * y2.z),
			(k0 * w) + (k1 * y2.w)
		);
	}

	// ================================
	// Operators
	// ================================

	TB_INLINE Quat& Quat::operator = (const Quat& a_Other)
	{
		x = a_Other.x;
		y = a_Other.y;
		z = a_Other.z;
		w = a_Other.w;

		return *this;
	}

	TB_INLINE Quat& Quat::operator=(const Mat4x4& a_Other)
	{
		FromMatrix(a_Other);

		return *this;
	}

	TB_INLINE Quat Quat::operator - () const
	{
		return Quat(-x, -y, -z, w);
	}

	TB_INLINE Quat Quat::operator * (const Quat& a_Other) const
	{
		/*
		_v[W]*p._v[X] + _v[X]*p._v[W] + _v[Y]*p._v[Z] - _v[Z]*p._v[Y], 
		_v[W]*p._v[Y] - _v[X]*p._v[Z] + _v[Y]*p._v[W] + _v[Z]*p._v[X], 
		_v[W]*p._v[Z] + _v[X]*p._v[Y] - _v[Y]*p._v[X] + _v[Z]*p._v[W] 
		_v[W]*p._v[W] - _v[X]*p._v[X] - _v[Y]*p._v[Y] - _v[Z]*p._v[Z], */

		return Quat(
			(w * a_Other.x) + (x * a_Other.w) + (y * a_Other.z) - (z * a_Other.y),
			(w * a_Other.y) - (x * a_Other.z) + (y * a_Other.w) + (z * a_Other.x),
			(w * a_Other.z) + (x * a_Other.y) - (y * a_Other.x) + (z * a_Other.w),
			(w * a_Other.w) - (x * a_Other.x) - (y * a_Other.y) - (z * a_Other.z)
		);
	}

	TB_INLINE Vec3 Quat::operator * (const Vec3& a_Transform) const
	{
		// i'm not touching this magic
		// i might break something

		// Ogre 1.6.2 source

		// NVIDIA SDK implementation
		Vec3 uv, uuv;
		Vec3 qvec(x, y, z);
		uv = qvec.GetCrossProduct(a_Transform);
		uuv = qvec.GetCrossProduct(uv);
		uv *= (2.f * w);
		uuv *= 2.f;

		return a_Transform + uv + uuv;
	}

	TB_INLINE Quat Quat::operator * (float a_Value) const
	{
		return Quat(
			x * a_Value,
			y * a_Value,
			z * a_Value,
			w * a_Value
		);
	}

	TB_INLINE Quat& Quat::operator *= (const Quat& a_Other)
	{
		Quat old = *this;

		x = (a_Other.w * old.x) + (a_Other.x * old.w) + (a_Other.y * old.z) - (a_Other.z * old.y);
		y = (a_Other.w * old.y) - (a_Other.x * old.z) + (a_Other.y * old.w) + (a_Other.z * old.x);
		z = (a_Other.w * old.z) + (a_Other.x * old.y) - (a_Other.y * old.x) + (a_Other.z * old.w);
		w = (a_Other.w * old.w) - (a_Other.x * old.x) - (a_Other.y * old.y) - (a_Other.z * old.z);

		return *this;
	}

	TB_INLINE Quat& Quat::operator *= (float a_Value)
	{
		x *= a_Value;
		y *= a_Value;
		z *= a_Value;
		w *= a_Value;

		return *this;
	}

}